#!/usr/bin/env python3.6
# -*- coding: utf-8 -*-

import socket
import random
import threading
import rospy
from robotserver.msg import State
from robotserver.msg import Target

server_socket = None
client_socket = None
global robotState
robotState = State()

def spinThread():
    rospy.spin()

def stateCallback(msg):
    global robotState
    robotState = msg


def server():
    rospy.init_node('server_node', anonymous=True)
    rospy.Subscriber("/robot/state", State, stateCallback)
    target_pub = rospy.Publisher('/robot/target', Target, queue_size=1)
    
    add_thread = threading.Thread(target = spinThread)
    add_thread.start()

    rate = rospy.Rate(25) 

    # Build tcp socket
    try:
        myaddress = ('192.168.4.2', 1234)
        server_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        server_socket.bind(myaddress)
        server_socket.listen(1)
        print("Server ip and port are: ", myaddress)

        while not rospy.is_shutdown():
            try:
                print("waiting for connect...")
                client_socket, address = server_socket.accept()
                print("连接已建立:", address)

                byte_array = bytearray(7)
                while True and (not rospy.is_shutdown()):
                    byte_array = client_socket.recv(7)
                    if len(byte_array) == 0:
                        break
                    if byte_array[0] == 2:
                        print("Refresh......")
                        sendMsg = bytearray(4)
                        print('sendMsg, ready: ', robotState.isReady)
                        print('sendMsg, yaw: ', robotState.yaw)
                        print('sendMsg, pitch: ', robotState.pitch)
                        print('sendMsg, pitch: ', robotState.distance)
                        sendMsg[0] = (robotState.isReady).to_bytes(1, byteorder='big', signed=True)[0]
                        sendMsg[1] = (robotState.yaw).to_bytes(1, byteorder='big', signed=True)[0]
                        sendMsg[2] = (robotState.pitch).to_bytes(1, byteorder='big', signed=True)[0]
                        sendMsg[3] = (robotState.distance).to_bytes(1, byteorder='big', signed=True)[0]

                        client_socket.sendall(sendMsg)
                    else:
                        print("New Msg......")
                        receiveMsg = [int.from_bytes(bytes([byte]), 'big', signed=True) for byte in byte_array]
                        target_msg = Target()
                        target_msg.isAuto = bool(receiveMsg[0])
                        target_msg.isRandom = bool(receiveMsg[1])
                        target_msg.isLaunch = bool(receiveMsg[2])
                        target_msg.isRetrieve = bool(receiveMsg[3])
                        target_msg.yaw = receiveMsg[4]
                        target_msg.pitch = receiveMsg[5]
                        target_msg.distance = receiveMsg[6]
                        target_pub.publish(target_msg)
                    rate.sleep()


            except socket.error as e:
                print("发生Socket错误: " + str(e))
            finally:
                if client_socket:
                    try:
                        client_socket.close()
                        print("客户端连接已关闭")
                    except socket.error as e:
                        print("关闭客户端连接发生错误: " + str(e))

    except socket.error as e:
        print("server端发生错误: " + str(e))
    finally:
        if server_socket:
            try:
                server_socket.close()
            except socket.error as e:
                print("关闭ServerSocket发生错误: " + str(e))

if __name__ == '__main__':
    try:
        server()
    except rospy.ROSInterruptException:
        pass


